// --------------------------------------------------------------------------------------------------------------------
// <copyright file="TurretScanner.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Navigation.Motion
{
    using System;
    using System.Threading.Tasks;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Implements smooth turret scanning.
    /// </summary>
    public class TurretScanner
    {
        /// <summary>
        /// Size of a single step during turret scan.
        /// </summary>
        private double turretSweepStepsInRadians;

        /// <summary>
        /// Minimal angle in radians from where to start sweeps.
        /// </summary>
        private double turretSweepRangeInRadiansStart;

        /// <summary>
        /// Maximum angle in radians where to stop sweeps.
        /// </summary>
        private double turretSweepRangeInRadiansEnd;

        /// <summary>
        /// Automatically-settable flag indicating whether robot is currently in capture mode (will result in 
        /// temporary drive suspension)
        /// </summary>
        private bool turretRotationInProgress;

        /// <summary>
        /// Current Turret rotation speed in radians per second
        /// </summary>
        private double currentTurretPanRotationSpeed;

        /// <summary>
        /// Current turret pan angle in radians.
        /// </summary>
        private volatile float turretCurrentPanAngle;

        /// <summary>
        /// Turret rotation speed below which we'll consider Kinect as 'stable' and re-transmit.
        /// </summary>
        private double considerTurretStableIfRotatingSlowerThanThis;

        /// <summary>
        /// Position between TurretSweepPositionStart and TurretSweepPositionEnd where turret is rested while driving forward.
        /// </summary>
        private double turretDefaultHeading;

        /// <summary>
        /// Desired Turret rotation speed in radians per second.
        /// </summary>
        private double desiredTurretPanRotationSpeed;

        /// <summary>
        /// Delay between initiating sweeps.
        /// </summary>
        private int turretHoldPositionInMilliseconds;

        /// <summary>
        /// Initializes a new instance of the TurretScanner class.
        /// </summary>
        /// <param name="considerTurretStableIfRotatingSlowerThanThis">Turret rotation speed below which we'll consider Kinect as 'stable' and re-transmit.</param>
        /// <param name="turretDefaultHeading">Position between TurretSweepPositionStart and TurretSweepPositionEnd where turret is rested while driving forward.</param>
        /// <param name="desiredTurretPanRotationSpeed">Desired Turret rotation speed in radians per second.</param>
        /// <param name="turretSweepStepsInRadians">Size of a single step during turret scan.</param>
        /// <param name="turretSweepRangeInRadiansStart">Minimal angle in radians from where to start sweeps.</param>
        /// <param name="turretSweepRangeInRadiansEnd">Maximal angle in radians from where to start sweeps.</param>
        /// <param name="turretHoldPositionInMilliseconds">Delay between initiating sweeps.</param>
        public TurretScanner(
            double considerTurretStableIfRotatingSlowerThanThis,
            double turretDefaultHeading,
            double desiredTurretPanRotationSpeed,
            double turretSweepStepsInRadians,
            double turretSweepRangeInRadiansStart,
            double turretSweepRangeInRadiansEnd,
            int turretHoldPositionInMilliseconds)
        {
            this.considerTurretStableIfRotatingSlowerThanThis = considerTurretStableIfRotatingSlowerThanThis;
            this.turretDefaultHeading = turretDefaultHeading;
            this.desiredTurretPanRotationSpeed = desiredTurretPanRotationSpeed;
            this.turretSweepStepsInRadians = turretSweepStepsInRadians;
            this.turretSweepRangeInRadiansStart = turretSweepRangeInRadiansStart;
            this.turretSweepRangeInRadiansEnd = turretSweepRangeInRadiansEnd;
            this.turretHoldPositionInMilliseconds = turretHoldPositionInMilliseconds;
        }

        /// <summary>
        /// Gets a value indicating whether the turret is currently stable.
        /// </summary>
        public bool IsStable
        {
            get
            {
                return this.currentTurretPanRotationSpeed < this.considerTurretStableIfRotatingSlowerThanThis;
            }
        }

        /// <summary>
        /// Update turret rotation speed and angle.
        /// </summary>
        /// <param name="currentTurretPanRotationSpeed">Current turret rotation speed.</param>
        /// <param name="turretCurrentPanAngle">Current turret pan angle.</param>
        public void UpdateTurret(double currentTurretPanRotationSpeed, float turretCurrentPanAngle)
        {
            this.currentTurretPanRotationSpeed = currentTurretPanRotationSpeed;
            this.turretCurrentPanAngle = turretCurrentPanAngle;
        }

        /// <summary>
        /// Sends the turret position message
        /// </summary>
        /// <param name="heading">Turret target heading</param>
        /// <param name="duration">Duration of turn</param>
        /// <param name="setSequenceFn">Set servo sequence function.</param>
        public void SetServoSequence(double heading, uint duration, Action<double, uint> setSequenceFn)
        {
            if (heading < this.turretSweepRangeInRadiansStart)
            {
                heading = this.turretSweepRangeInRadiansStart;
            }
            else if (heading > this.turretSweepRangeInRadiansEnd)
            {
                heading = this.turretSweepRangeInRadiansEnd;
            }

            setSequenceFn(heading, duration);
        }

        /// <summary>
        /// Initiates turret scan
        /// </summary>
        /// <param name="setHeadingFn">Function to set turret heading.</param>
        /// <returns>Task on which to await completion.</returns>
        public async Task InitiateTurretScan(Action<double, uint> setHeadingFn)
        {
            // this is not really intended as a fail-safe synchronization, but more as a sanity defense against
            // multiple user-initiated scans or to prevent user from resetting scan by accidentally issuing a manual sweep
            // while an autonomous one is in progress
            if (this.turretRotationInProgress)
            {
                return;
            }

            this.turretRotationInProgress = true;

            uint duration = (uint)((Math.Abs(0 - this.turretCurrentPanAngle) / this.desiredTurretPanRotationSpeed) * MathConstants.MilliunitsPerUnit);
            setHeadingFn(0, duration);
            await this.SmoothScanSection(this.turretDefaultHeading, this.turretSweepRangeInRadiansStart, setHeadingFn, false);
            await this.SmoothScanSection(this.turretSweepRangeInRadiansStart, this.turretSweepRangeInRadiansEnd, setHeadingFn, true);
            await this.SmoothScanSection(this.turretSweepRangeInRadiansEnd, this.turretDefaultHeading, setHeadingFn, false);

            this.turretRotationInProgress = false;
        }

        /// <summary>
        /// Sweeps turret from a given start position to the end position.
        /// </summary>
        /// <param name="startPosition">Start pan position in radians.</param>
        /// <param name="endPostion">End pan position in radians.</param>
        /// <param name="setHeadingFn">Function to set turret heading.</param>
        /// <param name="useSteps">Flag indicating if steps are to be used</param>
        /// <returns>Wait-able task</returns>        
        private async Task SmoothScanSection(double startPosition, double endPostion, Action<double, uint> setHeadingFn, bool useSteps)
        {
            int iterations = 0;
            if (useSteps)
            {
                iterations = (int)(Math.Abs(((endPostion - startPosition) / this.turretSweepStepsInRadians)));
            }

            double curPosition = startPosition;
            double stepDirection = startPosition < endPostion ? 1 : -1;
            double stepVal = Math.Min(Math.Abs(endPostion - startPosition), Math.Abs(this.turretSweepStepsInRadians));
            double singlePanStep = stepVal * stepDirection;

            // we go one extra step to make sure we cover the 100% of the section in cases where section is not evenly divisible
            // by step sizes
            for (int i = 0; i < iterations + 1; ++i)
            {
                curPosition += singlePanStep;

                if (iterations == i)
                {
                    curPosition = endPostion;
                }

                uint duration = (uint)((Math.Abs(curPosition - this.turretCurrentPanAngle) / this.desiredTurretPanRotationSpeed) * MathConstants.MilliunitsPerUnit);
                this.SetServoSequence(curPosition, duration, setHeadingFn);
                await Task.Delay((int)(duration + this.turretHoldPositionInMilliseconds));
            }
        }
    }
}
